
lim1_min = -pi/2; lim1_max = pi/2; lim1 = lim1_max - lim1_min;
lim2_min = -pi/2; lim2_max = pi/2; lim2 = lim2_max - lim2_min;
lim3_min = -pi/2; lim3_max = pi/2; lim3 = lim3_max - lim3_min;
lim4_min = -pi/2; lim4_max = pi/2; lim4 = lim4_max - lim4_min;
lim5_min = -pi/2; lim5_max = pi/2; lim5 = lim5_max - lim5_min;

% 定义各个连杆和关节类型，默认为转动关节（可选为平移关节）
%       theta    d       a   alpha
L1=Link([   0    0       0   pi/2], 'standard'); L1.qlim = [lim1_min, lim1_max];
L2=Link([   0    0   0.045      0], 'standard'); L2.qlim = [lim2_min, lim2_max];
L3=Link([   0    0   0.045      0], 'standard'); L3.qlim = [lim3_min, lim3_max]; L2.offset = pi/2;
L4=Link([   0    0   0.045   pi/2], 'standard'); L4.qlim = [lim4_min, lim4_max];
L5=Link([   0    0       0      0], 'standard'); L5.qlim = [lim5_min, lim5_max];

robot=SerialLink([L1, L2, L3, L4, L5]); % 将5个关节组成机械臂
robot.name='5-DOF Robotic Arm';
% robot.display(); % 在命令行界面中展示机械臂的DH参数表
% view(3); % 解决robot.teach()和plot的索引超出报错
% robot.teach();
% robot.plot([0 pi/2 0 0 0]); % 使用运动学模型，配置关节转角theta，展现机械臂的位姿

% joint(:,1) = linspace(-pi/2, pi/2);
% joint(:,2) = linspace(-pi/2, pi/2);
% joint(:,3) = linspace(-pi/2, pi/2);
% joint(:,4) = linspace(-pi/2, pi/2);
% joint(:,5) = linspace(-pi/2, pi/2);
% robot.plot(joint, 'jointdiam', '1', 'fps', 100, 'trail', 'r-')

% 使用蒙特卡洛法绘制机械臂的工作空间
N = 15000;
theta1 = lim1_min + lim1 * rand(N, 1);
theta2 = lim2_min + lim2 * rand(N, 1);
theta3 = lim3_min + lim3 * rand(N, 1);
theta4 = lim4_min + lim4 * rand(N, 1);
theta5 = lim5_min + lim5 * rand(N, 1);

for n = 1:N
    theta = [theta1(n),theta2(n), theta3(n), theta4(n), theta5(n)];
    workspace = robot.fkine(theta);
    plot3(workspace.t(1), workspace.t(2), workspace.t(3), 'b.', 'MarkerSize', 1);
    hold on;
end

robot.plot(theta);
robot.teach();